import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node, ComposableNodeContainer
from launch_ros.descriptions import ComposableNode

def generate_launch_description():
    bringup_dir = get_package_share_directory("binocular_driver")
    params_file = LaunchConfiguration("params_file")

    declare_params_file_cmd = DeclareLaunchArgument(
        "params_file",
        default_value=os.path.join(bringup_dir, "config", "launch_params.yaml"),
        description="config file for video_sim_cam",
    )

    container = ComposableNodeContainer(
            name='image_container',
            namespace='',
            package='rclcpp_components',
            executable='component_container',
            composable_node_descriptions=[
                ComposableNode(
                    package='binocular_driver',
                    plugin='binocular_driver::binocular_driver_node',
                    name='binocular_driver',
                    remappings=[],
                    parameters=[params_file],
                    extra_arguments=[]),
                ComposableNode(
                    package='usb_cam',
                    plugin='usb_cam::UsbCamNode',
                    name='usb_cam_node',
                    remappings=[
                        ("/image_raw", "/usb_cam_raw/image_raw"),
                        ("/image_raw/compressed", "/usb_cam_raw/image_raw/compressed"),
                        ("/image_raw/compressedDepth", "/usb_cam_raw/image_raw/compressedDepth"),
                        ("/image_raw/theora", "/usb_cam_raw/image_raw/theora"),
                        ("/camera_info", "/usb_cam_raw/camera_info"),
                    ],
                    parameters=[params_file],
                    extra_arguments=[])
            ],
            output='both',
    )
    

    # node = Node(
    #     package="binocular_driver",
    #     executable="binocular_driver_node",
    #     name="binocular_driver",
    #     output="screen",
    #     parameters=[params_file],
    # )

    
    # usb_cam_node = Node(
    #     package="usb_cam",
    #     executable="usb_cam_node_exe",
    #     name="usb_cam_node",
    #     output="screen",
    #     parameters=[params_file],
    #     remappings=[
    #         ("/image_raw", "/usb_cam_raw/image_raw"),
    #         ("/image_raw/compressed", "/usb_cam_raw/image_raw/compressed"),
    #         ("/image_raw/compressedDepth", "/usb_cam_raw/image_raw/compressedDepth"),
    #         ("/image_raw/theora", "/usb_cam_raw/image_raw/theora"),
    #         ("/camera_info", "/usb_cam_raw/camera_info"),
    #     ],
    # )
    # 添加 stereo_image_proc 的 disparity 节点
    
    disparity_node = Node(
        package="stereo_image_proc",
        executable="disparity_node",
        name="disparity_node",
        output="screen",
        parameters=[params_file],
        remappings=[
            ("left/image_rect", "/camera/left/image_raw"),
            ("left/camera_info", "/camera/left/camera_info"),
            ("right/image_rect", "/camera/right/image_raw"),
            ("right/camera_info", "/camera/right/camera_info"),
        ]
    )

    # # 添加 stereo_image_proc 的 point_cloud2 节点
    point_cloud2_node = Node(
        package="stereo_image_proc",
        executable="point_cloud_node",
        name="point_cloud2_node",
        # output="screen",
        parameters=[params_file],
        remappings=[
            ("left/image_rect_color", "/camera/left/image_raw"),
            ("left/camera_info", "/camera/left/camera_info"),
            ("right/camera_info", "/camera/right/camera_info"),
            ("disparity", "/disparity"),
            ("points2","/points2")
        ]
    )
    tf_broadcaster_node = Node(
        package="binocular_driver",
        executable="tf_broadcaster_node",  
        name="camera_tf2_broadcaster",
        output="screen",
        parameters=[params_file],
    )
    launch_des = LaunchDescription()

    launch_des.add_action(declare_params_file_cmd)
    launch_des.add_action(container)
    # launch_des.add_action(usb_cam_node)
    launch_des.add_action(disparity_node)
    launch_des.add_action(point_cloud2_node)
    launch_des.add_action(tf_broadcaster_node)

    return launch_des
